#include "cmd_task.h"
#include "math.h"
#include "wheel_group.h"
#include "referee.h"
#include "monitor.h"

#define PI 3.14159265

float cmd_power_factor = 0;

void cmd_chassis_refresh();

void start_cmd_task(void *argument)
{
	//uint8_t soft_start_mode;
	//vTaskDelay(500);
	chassis_calculate_init();
	while(1)
	{
		//wheel_group_controller_execute();
		cmd_chassis_refresh();
		HAL_Delay(1);
	}
}

void cmd_chassis_refresh()
{
	float cmd_vx = 0;
	float cmd_vy = 0;
	float cmd_vz = 0;
//	cmd_vx = (float)((int16_t)dbus_ctrl_data.channel.CH[2] - 0x400) / 660;
//	cmd_vy = (float)((int16_t)dbus_ctrl_data.channel.CH[3] - 0x400) / 660;
//	cmd_vz = (float)((int16_t)dbus_ctrl_data.channel.rolling_wheel - 0x400) / 660;
	float cmd_sum = 1;
	
	float power_target = 40;
	if(monitor_referee_online)
	{
		power_target = 0.85 * (referee_game_robot_status.chassis_power_limit);
	}
	
	cmd_sum = fabs((float)interface_speed_cmd.vx) + fabs((float)interface_speed_cmd.vy) + fabs((float)interface_speed_cmd.vz);
	if(cmd_sum > 10)
	{
		cmd_vx = (float)interface_speed_cmd.vx / cmd_sum;
		cmd_vy = (float)interface_speed_cmd.vy / cmd_sum;
		cmd_vz = (float)interface_speed_cmd.vz / cmd_sum;
		if(monitor_referee_online && monitor_power_meter_online)
		{
			cmd_power_factor += ((power_target - power_meter_data.power) * 0.003);
		}
		else if (monitor_referee_online)
		{
			cmd_power_factor += ((power_target - referee_power_heat_data.chassis_power) * 0.0025);
		}
		else if (monitor_power_meter_online)
		{
			cmd_power_factor += ((100 - power_meter_data.power) * 0.003);
		}
		else
		{
			cmd_power_factor = 10;
		}
	}
	else
	{
		cmd_power_factor = 0;
	}
	if(cmd_power_factor > 450)
	{
		cmd_power_factor = 450;
	}
	chassis_calculate_wheel_group(cmd_vy, - cmd_vx, cmd_vz, cmd_power_factor);
}
